﻿
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>

#define INS_NODE_DATA   //实例化仿真接口数据 只能在该cpp文件定义

#define CONFIG_FILE "C:/Users/Administrator/Desktop/LogSimDemo/Simulation/config.ini"
//仿真环境头文件
#include "simEnv.h"
#include "nodeData.h"

//仿真输入信号头文件
#include "Reader/VehicleMessage.h"
#include "Reader/RoadMessage.h"

//算法头文件
#include "Algorithm/Demo/AlgorithmIF.h"
#include "Algorithm/Sil/SilLabel/SilLabelIF.h"
#include "Algorithm/Demo/SystemTime.h"

//仿真存储信号头文件
#include "Recorder/Sil_RecorderIF.h"
#include "Algorithm/Sil/Algorithm_RecorderIF.h"


void SimEnv::simEnvConfig(std::string& configFileName, uint32_t& dataPoolSize)
{
    configFileName = CONFIG_FILE;
    dataPoolSize = DATA_POOL_SIZE;
}

void SimEnv::calcDriveSignalTimeQueue(const std::vector<uint64_t>& originalTimeQueue, const std::vector<double>& originalDataQueue, std::vector<uint64_t>& desiredTimeQueue, std::vector<double>& desiredDataQueue)
{
    for (uint32_t index = 0U, size = originalTimeQueue.size(); index < size; index++)
    {
        if (0 == index)
        {
            desiredTimeQueue.push_back(originalTimeQueue[index]);
            desiredDataQueue.push_back(originalDataQueue[index]);
        }
        else if (std::abs(desiredDataQueue.back() - originalDataQueue[index]) >= 1U)
        {
            desiredTimeQueue.push_back(originalTimeQueue[index]);
            desiredDataQueue.push_back(originalDataQueue[index]);
        }
        else
        {
            //do nothing
        }
    }

    std::cout<<"originalTimeQueue size = "<< originalTimeQueue.size() <<std::endl;
    std::cout<<"desiredTimeQueue size = "<< desiredTimeQueue.size() <<std::endl;
}

void SimEnv::add_NodeSignal(std::vector<std::string>& nodeSignals)
{
    add_RoadMessage_SimSignal(nodeSignals);
    add_VehicleMessage_SimSignal(nodeSignals);

    std::cout<<"read signal number = "<<nodeSignals.size() <<std::endl;
}

void SimEnv::init_NodeData(void)
{
    init_RoadMessage_Data(NodeData::roadMessage, DATA_POOL_SIZE);
    init_VehicleMessage_Data(NodeData::vehicleMessage, DATA_POOL_SIZE);
}

void SimEnv::mapping_NodeData(const SimSignalData& simSignalData, const uint32_t index)
{
    mapping_RoadMessage_Data(simSignalData, NodeData::roadMessage, index);
    mapping_VehicleMessage_Data(simSignalData, NodeData::vehicleMessage, index);
}

void SimEnv::algorithm_init(void)
{
    Algorithm::algorithmInit();
}

uint64_t g_driveSignal = 0U;
void SimEnv::algorithm_proc(const uint32_t readerIndex, const double driveSignal, const uint64_t timeStamp, const uint32_t dataFrameIndex, std::vector<Mf4Recorder::TimeSlice>& timeSlice)  
{
    Algorithm::set_timeStampSim(timeStamp); 

    g_driveSignal = driveSignal;
    
    NodeData::SIL_roadMessage = NodeData::roadMessage[readerIndex]; //用于记录数据
   
    Algorithm::algorithmProc(NodeData::roadMessage[readerIndex], NodeData::vehicleMessage[readerIndex],
                             NodeData::algorithmMessage);

    timeSlice = Algorithm::getLabel();
    Algorithm::clearLabel();
}

void SimEnv::algorithm_destroy(void)
{
    Algorithm::algorithmDestroy();
}

void SimEnv::addSignal_recorder(uint32_t& port, std::vector<Mf4Recorder::SignalBaseInfo>& recorderSignal)
{
    port = 100U; //非零即可

    addSignal_Sil_recorder(recorderSignal);
    addSignal_Algorithm_recorder(recorderSignal);
    
    std::cout<<"recorderSignal size = "<<recorderSignal.size()<<std::endl;
}

void startSimulation(void)
{
    auto startTime = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();

    std::cout<<"start simulation"<<std::endl;

    SimEnv ins_SimEnv;
    SimDriveImpl::SimEnvRegister(&ins_SimEnv);

    auto endTime = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
	std::cout<<"Simulation run time = "<<(endTime - startTime)<<"s"<<std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    system("pause");
}
